// ROS
#include <ros/ros.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>

namespace ros_control_boilerplate
{
  static const double SEC_PER_TRAJ_POINT = 3.0; // 两点之间的执行时间
  static const std::size_t TRAJ_POINTS = 10;     // 生成的轨迹点数量

  class TestTrajectory
  {
  public:
    /**
   *  Constructor
   */
    TestTrajectory()
        : nh_private_("~")
    {
      nh_private_.getParam("trajectory_controller", trajectory_controller);
      if (trajectory_controller.empty())
      {
        ROS_FATAL_STREAM_NAMED(
            "test_trajectory",
            "No joint trajectory controller parameter found on the parameter server");
        exit(-1);
      }
      ROS_INFO_STREAM_NAMED("test_trajectory",
                            "Connecting to controller " << trajectory_controller);

      // create the action client
      // true causes the client to spin its own thread
      actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> action_client(
          trajectory_controller + "/follow_joint_trajectory/", true);

      ROS_INFO_NAMED("test_trajetory", "Waiting for action server to start.");
      // wait for the action server to start
      action_client.waitForServer(); // will wait for infinite time

      ROS_INFO_NAMED("test_trajetory", "Action server started, sending goal.");

      // send a goal to the action
      control_msgs::FollowJointTrajectoryGoal goal;
      goal.trajectory = createTrajectory();
      std::cout << "Trajectry:\n"
                << goal.trajectory << std::endl;
      action_client.sendGoal(goal);

      // Wait for the action to return
      double wait_extra_padding = 2; // time to wait longer than trajectory itself
      bool finished_before_timeout = action_client.waitForResult(
          ros::Duration(goal.trajectory.points.back().time_from_start.toSec() + wait_extra_padding));

      if (finished_before_timeout)
      {
        actionlib::SimpleClientGoalState state = action_client.getState();
        ROS_INFO_NAMED("test_trajetory", "Action finished: %s", state.toString().c_str());
      }
      else
        ROS_INFO_NAMED("test_trajetory", "Action did not finish before the time out.");

      ROS_INFO_STREAM_NAMED("test_trajectory", "TestTrajectory Finished");
    }

    /**
   *  Create random trajectory
   */
    trajectory_msgs::JointTrajectory createTrajectory()
    {
      std::vector<std::string> joint_names;
      double min_joint_value = -3.14;
      double max_joint_value = 3.14;

      // Get joint names
      nh_private_.getParam(trajectory_controller + "/joints", joint_names);
      if (joint_names.size() == 0)
      {
        ROS_FATAL_STREAM_NAMED(
            "init",
            "Not joints found on parameter server for controller, did you load the proper yaml file?"
                << " Namespace: " << nh_private_.getNamespace() << "/hardware_interface/joints");
        exit(-1);
      }

      nh_private_.getParam("min_joint_value", min_joint_value);
      nh_private_.getParam("max_joint_value", max_joint_value);
      ROS_INFO_STREAM_NAMED("test_trajectory", "Creating trajectory with joint values from "
                                                   << min_joint_value << " to " << max_joint_value);

      // Create header
      trajectory_msgs::JointTrajectory trajectory;
      trajectory.header.stamp = ros::Time::now();
      trajectory.joint_names = joint_names;

      // Create trajectory with x points
      trajectory.points.resize(TRAJ_POINTS);
      for (std::size_t i = 0; i < TRAJ_POINTS; ++i)
      {
        trajectory.points[i].positions.resize(joint_names.size());
        // for each joint
        for (std::size_t j = 0; j < joint_names.size(); ++j)
        {
          trajectory.points[i].positions[j] = dRand(min_joint_value, max_joint_value);
          trajectory.points[i].time_from_start = ros::Duration(i * SEC_PER_TRAJ_POINT + 1);
        }
      }

      return trajectory;
    }

    /**  Get random number */
    double dRand(double dMin, double dMax)
    {
      double d = (double)rand() / RAND_MAX;
      return dMin + d * (dMax - dMin);
    }

  private:
    // A shared node handle
    ros::NodeHandle nh_private_;

    // A string containing the 'trajectory_controller' parameter value
    std::string trajectory_controller;
  }; // end class

  // Create boost pointers for this class
  typedef boost::shared_ptr<TestTrajectory> TestTrajectoryPtr;
  typedef boost::shared_ptr<const TestTrajectory> TestTrajectoryConstPtr;

} // namespace ros_control_boilerplate

int main(int argc, char **argv)
{
  ros::init(argc, argv, "test_trajectory");
  ROS_INFO_STREAM_NAMED("test_trajectory", "Starting TestTrajectory...");

  // Allow the action server to recieve and send ros messages
  ros::AsyncSpinner spinner(2);
  spinner.start();

  ros_control_boilerplate::TestTrajectory server;

  ROS_INFO_STREAM_NAMED("test_trajectory", "Shutting down.");
  ros::shutdown();

  return 0;
}
